#ifndef REACTIVE_BEHAVIOR_H_
#define REACTIVE_BEHAVIOR_H_

#include <libplayerc++/playerc++.h>
#include <libplayerinterface/player.h>
#include <core/synced_current_pose.h>

using PlayerCc::PlayerClient;
using PlayerCc::Position2dProxy;
using PlayerCc::SonarProxy;

using PlayerCc::rtod;
using PlayerCc::dtor;

class ReactiveBehavior {
private:
  // How much time, in seconds, the 
  // robot has to reach the next pose,
  // before it fails.
  static const int kTimeout = 100;                    

  // Minimum distance, in meters,
  // between robot and goal, that can
  // be considered as an arrival criteria.
  static const double kAtGoalThreshold = 0.35;         

  // Maximum range, in meters, before 
  // an obstacle gets detected and robot stops.
  static const double kObstacleThreshold = 0.27;

  // Maximum distance, in meters, before 
  // robot stops rotating away from obstacle
  // and starts rotating towards.
  // This defines how close will the robot
  // be to the obstacle's contour during a WallFollow. 
  static const double kWallFollowThreshold = 0.35;

  // Maximum translational speed,
  // in meters/sec.
  static const double kMaxForwardSpeed = 0.3;
  
  // Maximum R/L Wall Follow rotational 
  // speed in radians/sec.
  // Obs: positive turning speeds will
  // cause counter-clockwise rotation (left turn).
  static const double kMaxRotSpeed = 3.141592653589793238462643 / 4;  

  static const double kGoalSeekRotScaler = 0.6;  
  static const double kWallFollowRotScaler = 0.6;  
  static const double kForwardSpeedScaler = 0.19;
                                              
  // TODO(Adiwardana) empirically adjust these constants 
  // above for the non-simulated platform.

  PlayerClient *player_client_;
  Position2dProxy *position2d_proxy_;
  SonarProxy *sonar_proxy_;
  SyncedCurrentPose *synced_current_pose_;

  // Forward speed given sonar readings.
  double ComputeForwardSpeed();

  // Check for the presence of an obstacle in the direction of the goal.
  bool ObstaclesInWay(const double &goal_angle); 

  // Rotation angle for Goal Seek state.
  double ComputeGoalSeekRot(const double &goal_angle);

  // Rotation angle for Left Wall Follow.
  double ComputeLWFRot();

  // Rotation angle for Right Wall Follow.
  double ComputeRWFRot();

  // Random rotation.
  void RotateRandomly();
  
  // Check if robot is stuck.
  bool IsRobotStuck(player_point_2d position);

public:
  ReactiveBehavior(PlayerClient *, 
                   Position2dProxy *, 
                   SonarProxy *, 
                   SyncedCurrentPose *);

  // Moves the robot toward the goal position, while going around
  // any obstacles it encounters, until it either reaches the 
  // goal position, returning true, or it fails, returning false.
  // For instance, failure could be due to a large obstacle that
  // forces the robot to move far away from the goal position 
  // or even due to a complete path obstruction.
  bool MoveToGoalAvoidingObstacles(const player_point_2d &goal_position);

  // Rotates the robot, so it's orientation corresponds to the given
  // angle, in the local coordinate system.
  void RotateTo(const double &angle);
};

#endif
